// this is the header file that tells the compiler what pins and ports, etc.
 // are available on this chip.
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <stdio.h>
 #include <stdint.h>
 #include "mercury.h"
 #include "motorControl.h"
 #include "encoder.h"
 #include "sensors.h"
 
int main(void) {
   // initialize the direction of PORTD #6 to be an output
	uint8_t count;
	uint8_t warning;

    ioinit(); //Setup IO pins and defaults  
	init_Motor();
	init_pwm();
    init_adc(); //initialize ADC
	init_Encoders();

	sensorArray[IdByte]='V';

	sei();
	//delay_ms(20);
	output_high(PORTB,Green);
	//delay_ms(100);
	output_high(PORTB,Yellow);
	//delay_ms(100);
	output_high(PORTB,Red);
	//delay_ms(100);
	output_low(PORTB,Yellow);
	output_low(PORTB,Red);

	set(TCCR0A , WGM01); // enable CTC on timer 0
	set(TIMSK0 , OCIE0A);//enable timer intrupt

	set(TCCR1B , WGM12); // enable CTC on timer 1
	set(TIMSK1 , OCIE1A);//enable timer intrupt

	OCR0A=0x98;
	OCR1A=0x98;
	
	LOS=640;
	startTimer1;

  while(1){
  		output_high(PORTB,Green);
	  	count = 0;
		do{
			if(UCSR0A & (1<<RXC0)){ // chagne to USART0!
				DataPacket[count] = uart_getdata();
				//printf("Char %c",DataPacket[count]);
				if(DataPacket[IdByte] == 'V'){count++;}
				else{count=0;}
			}//if
			warning = read_sensor();
			if(warning && !Override){
            lowBrake();
			//printf("Sensor Stop %d",warning);
        	}
		}while(count <9);
		if(DataPacket[CommandByte]&0x80){
		Override=1;
		}else{
		Override=0;
		}
		output_low(PORTB,Green);
		LOS=640;
		output_low(PORTB,Red);
		startTimer1;
		Move();
		sensorArray[SensorWarning]=warning;
        SendSensorData();
        Debug();
      
	}//while(1)	

 }//main

void Move(void){

if(DataPacket[IdByte]=='V'){
	if (DataPacket[LeftSpeed] >= 'A' && DataPacket[LeftSpeed] <= 'Z'){ 
		
		if (DataPacket[RightSpeed] >= 'A' && DataPacket[RightSpeed] <= 'Z'){
			CalibrateNow = 1;
			forward(DataPacket[LeftSpeed],DataPacket[RightSpeed]);
		} else if (DataPacket[RightSpeed] >= 'a' && DataPacket[RightSpeed] <= 'z'){
			CalibrateNow = 0;
			rightTurn(DataPacket[LeftSpeed],DataPacket[RightSpeed]);
		}

	} else if (DataPacket[LeftSpeed] >= 'a' && DataPacket[LeftSpeed] <= 'z'){

		if (DataPacket[RightSpeed] >= 'A' && DataPacket[RightSpeed] <= 'Z'){
			CalibrateNow=0;
			leftTurn(DataPacket[LeftSpeed],DataPacket[RightSpeed]);
		} else if (DataPacket[RightSpeed] >= 'a' && DataPacket[RightSpeed] <= 'z'){
			CalibrateNow=1;
			reverse(DataPacket[LeftSpeed],DataPacket[RightSpeed]);
		}

	} else if(DataPacket[LeftSpeed] == '!' && DataPacket[RightSpeed] == '!'){
		CalibrateNow=0;
		//adjustable Breaking
		if(DataPacket[CommandByte]&0x08){
			stop();
		}else{
			lowBrake();
		}
		sensorArray[LeftEncoderH]= turnsL >>8;
		sensorArray[LeftEncoderL]= turnsL & 0xff;
		sensorArray[RightEncoderH]= turnsR >>8;
		sensorArray[RightEncoderL]= turnsR & 0xff;
		printf("Brake or Stop , turnsL = %d, turnsR = %d\n", turnsL, turnsR);
		turnsL=0;
		turnsR=0;

	} else if ((DataPacket[LeftSpeed] == '$' && DataPacket[RightSpeed] == '$')){
		CalibrateNow=0;
	//do nothing is a sync packet
	} else {
		stop();
	}
	
	TimerCount=((DataPacket[TimerByte]-64)*8);
	output_high(PORTB,Yellow);
	startTimer0;
    
}
}

ISR(TIMER0_COMPA_vect){
	TimerCount--;
	if(TimerCount==0){
		stopTimer0;
		output_low(PORTB,Yellow);
		if(DataPacket[CommandByte]&0x08){
		stop();
		}else{
		lowBrake();
		}
		sensorArray[LeftEncoderH]= turnsL >>8;
		sensorArray[LeftEncoderL]= turnsL & 0xff;
		sensorArray[RightEncoderH]= turnsR >>8;
		sensorArray[RightEncoderL]= turnsR & 0xff;
		if(DataPacket[CommandByte]&0x01 && CalibrateNow){
		Calibrate();
		printf("CalByte %d" ,CalByte);
		}
		//printf("Stop!, turnsL = %d, turnsR = %d, oldstate = %x, newstate = %x\n", turnsL, turnsR, oldstate, newstate);
		turnsL = 0;
		turnsR = 0;
	}
}

ISR(TIMER1_COMPA_vect){
	LOS--;
	if(LOS==0){
		LOS=640;
		output_high(PORTB,Red);
		printf("LOS!\n");
 	}
}

